/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 占位处理方法文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H

#include <costmap_2d/msg_struct.h>

namespace costmap_2d {
/**
*@brief 计算占位的极限距离
*@param const std::vector<costmap_2d::Point>& footprint 占位
*@param double& min_dist 最小距离
*@param double& max_dist 最大距离
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
void calculateMinAndMaxDistances(
    const std::vector<costmap_2d::Point>& footprint, double& min_dist,
    double& max_dist);
/**
*@brief Point32 to Point
*@param costmap_2d::Point32 pt 原始值
*@return costmap_2d::Point 转换后的值
*@author: tuyinan
*@version: 2022-04-xx
*/
costmap_2d::Point toPoint(costmap_2d::Point32 pt);

/**
*@brief Point to Point32
*@param costmap_2d::Point pt 原始值
*@return costmap_2d::Point32 转换后的值
*@author: tuyinan
*@version: 2022-04-xx
*/
costmap_2d::Point32 toPoint32(costmap_2d::Point pt);

/**
*@brief std::vector<costmap_2d::Point> to costmap_2d::Polygon
*@param std::vector<costmap_2d::Point> pts 原始值
*@return costmap_2d::Polygon 转换后的值
*@author: tuyinan
*@version: 2022-04-xx
*/
costmap_2d::Polygon toPolygon(std::vector<costmap_2d::Point> pts);

/**
*@brief std::vector<std::vector<double> > to std::vector<costmap_2d::Point>
*@param std::vector<std::vector<double> > polygon 原始值
*@return std::vector<costmap_2d::Point> 转换后的值
*@author: tuyinan
*@version: 2022-04-xx
*/
std::vector<costmap_2d::Point> toPointVector(
    std::vector<std::vector<double> > polygon);

/**
*@brief 根据朝向更新占位
*@param double x 机器人的位姿x
*@param double y 机器人的位姿y
*@param double theta 机器人的转角 theta
*@param const std::vector<costmap_2d::Point>& footprint_spec 转换前的占位
*@param std::vector<costmap_2d::Point>& oriented_footprint 转换后的占位
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
void transformFootprint(double x, double y, double theta,
                        const std::vector<costmap_2d::Point>& footprint_spec,
                        std::vector<costmap_2d::Point>& oriented_footprint);
/**
*@brief 膨胀占位
*@param std::vector<costmap_2d::Point>& footprint 占位
*@param double padding 膨胀半径
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
void padFootprint(std::vector<costmap_2d::Point>& footprint, double padding);

/**
*@brief 机器人半径到占位
*@param double radius 机器人半径
*@return std::vector<costmap_2d::Point> 占位
*@author: tuyinan
*@version: 2022-04-xx
*/
std::vector<costmap_2d::Point> makeFootprintFromRadius(double radius);

}  // end namespace costmap_2d

#endif  // COSTMAP_2D_FOOTPRINT_H
